#include <iostream>
#include "Astar.h"
#include <ros/ros.h>
#include <astar_pkg/MovePoint.h>
#include <math.h>
#include <thread>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

#include <moveit_visual_tools/moveit_visual_tools.h>
#include <tf/LinearMath/Quaternion.h>

using namespace std;

struct location
{
	int x;
	int y;
}location;

void PointCallback(const astar_pkg::MovePoint::ConstPtr &msg)
 {
    location.x=msg->x;
	location.y=msg->y;
	ROS_INFO("Listener:x=%d,y=%d",location.x,location.y);
 }

int main(int argc, char ** argv)

{
	ros::init(argc, argv, "listener");
	ros::NodeHandle n;
	ros::Subscriber sub=n.subscribe("point_info",1,PointCallback);
//   ros::init(argc, argv, "MoveGroupInterface_To_Kinetic");
//   ros::NodeHandle n;
//   ros::Subscriber sub=n.subscribe("point_info",100,PointCallback);

//  // Start a thread
//   ros::AsyncSpinner spinner(1);
//   spinner.start();

//    // Define the planning group name
//   static const std::string PLANNING_GROUP = "manipulator_i5";


//   // Create a planning group interface object and set up a planning group
//   moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
//   move_group.setPoseReferenceFrame("base_link");


//   // Create a planning scene interface object
//   moveit::planning_interface::PlanningSceneInterface planning_scene_interface;


//   // Create a robot model information object
//   const robot_state::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);


//   // Create an object of the visualization class
//   namespace rvt = rviz_visual_tools;
//   moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
//   visual_tools.deleteAllMarkers();


//   // Load remote control tool
//   visual_tools.loadRemoteControl();


//   // Create text
//   Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
//   text_pose.translation().z() = 1.2;
//   visual_tools.publishText(text_pose, "AUBO Demo", rvt::RED, rvt::XLARGE);
//   // Text visualization takes effect
//   visual_tools.trigger();


//   // Get the coordinate system of the basic information
//   ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str());


//   // Get the end of the basic information
//   ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());

//   std::vector<double> home_position;
//   home_position.push_back(1.023208);
//   home_position.push_back(0.438791);
//   home_position.push_back(2.614588);
//   home_position.push_back(0.6053415);
//   home_position.push_back(1.571933);
//   home_position.push_back(-2.119505);
//   move_group.setJointValueTarget(home_position);
//   move_group.move();


//   // Visual terminal prompt (blocking)
//   visual_tools.prompt("Press 'next'1 in the RvizVisualToolsGui window to start the demo");

//  // Set the target pose , RPY mode (rotation around the reference axis X, Y, Z)
//   tf::Quaternion q;
//   q.setRPY(3.14,0,-1.57);       //radian

//   geometry_msgs::Pose target_pose1;
//   target_pose1.position.x = 0.2;
//   target_pose1.position.y = 0.4;
//   target_pose1.position.z = 0.39;
//   target_pose1.orientation.x = q.x();
//   target_pose1.orientation.y = q.y();
//   target_pose1.orientation.z = q.z();
//   target_pose1.orientation.w = q.w();

//   move_group.setPoseTarget(target_pose1);

//   // Call the planner for planning calculations Note: This is just planning
//   moveit::planning_interface::MoveGroupInterface::Plan my_plan;
//   bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
//   ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "Success" : "FAILED");


//   // visual planning path in Rviz
//   visual_tools.deleteAllMarkers();
//   visual_tools.publishAxisLabeled(target_pose1, "pose1");
//   visual_tools.publishText(text_pose, "AUBO Pose Goal Example1", rvt::RED, rvt::XLARGE);
//   // Parameter 1 (trajectory_): path information
//   // Parameter 2 (JointModelGroup): Joint angle information and arm model information of the initial pose
//   visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
//   visual_tools.trigger();


//   // Perform planning actions
//   move_group.execute(my_plan);

    

	//初始化地图，用二维矩阵代表地图，1表示障碍物，0表示可通
	vector<vector<int>> maze = {
	   // 0  1  2  3  4  5  6  7  8  9 10  11
		{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
		{ 1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1 },
		{ 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1 },
		{ 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1 },
		{ 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1 },
		{ 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1 },
		{ 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1 },
		{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }
	};
	Astar astar;
	astar.InitAstar(maze);
    ros::spin();
	if(maze[location.x][location.y]==0)
	{
		maze[location.x][location.y]=1;
	}
	
	
	//设置起始和结束点
	Point start(1, 1);
	Point end(6, 10);
	//A*算法找寻路径
	list<Point *> path = astar.GetPath(start, end, false);

	// std::thread socket_thread(SocketThread);
	// socket_thread.detach();
	// // ros::MultiThreadedSpinner spinner1(1); // Use 4 threads
    // // spinner1.spin(); 

	//打印
    // geometry_msgs::Pose target_pose;
    // std::vector<geometry_msgs::Pose> waypoints;
	// target_pose = target_pose1;
	for (auto &p : path)
		{
		cout << '(' << p->x << ',' << p->y << ')' << endl;
        // target_pose.position.x = p->x /10.0 ;
        // target_pose.position.y = p->y /10.0 ;
        // waypoints.push_back(target_pose);
		// cout<<"机械臂走的点 "<<target_pose.position.x <<","<<target_pose.position.y<<endl;
		}

//   // Reduce the speed of the robot arm by the scaling factor of the maximum speed of each joint. Please note that this is not the speed of the final effector.
//   move_group.setMaxVelocityScalingFactor(0.5);

//   // We want the Cartesian path to be interpolated at a resolution of 1 cm.
//   moveit_msgs::RobotTrajectory trajectory;
//   const double jump_threshold = 0.0;           //(The jump threshold is set to 0.0)
//   const double eef_step = 0.01;                //(interpolation step)



//   // Calculate Cartesian interpolation path: return path score (0~1, -1 stands for error)
//   double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
//   ROS_INFO_NAMED("tutorial", "Visualizing plan  (Cartesian path) (%.2f%% acheived)", fraction * 100.0);


//   // Visualize the plan in RViz
//   visual_tools.deleteAllMarkers();
//   visual_tools.publishText(text_pose, "AUBO Joint Space Goal Example4", rvt::RED, rvt::XLARGE);
//   visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
//   for (std::size_t i = 0; i < waypoints.size(); ++i)
//   {
//     visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
//   }
//   visual_tools.trigger();


//   // Move to the home point position
//   my_plan.trajectory_= trajectory;
//   move_group.execute(my_plan);


//   // Clear path constraint
//   move_group.setJointValueTarget(home_position);
//   move_group.move();

//    // END_TUTORIAL
//    ros::shutdown();
	// system("pause");
	return 0;
}

void Astar::InitAstar(std::vector<std::vector<int>> &_maze)
{
	maze = _maze;
}
 
int Astar::calcG(Point *temp_start, Point *point)
{
	int extraG = (abs(point->x - temp_start->x) + abs(point->y - temp_start->y)) == 1 ? kCost1 : kCost2;
	int parentG = point->parent == NULL ? 0 : point->parent->G; //如果是初始节点，则其父节点是空
	return parentG + extraG;
}
 
int Astar::calcH(Point *point, Point *end)
{
	//用简单的欧几里得距离计算H，这个H的计算是关键，还有很多算法，没深入研究^_^
	return sqrt((double)(end->x - point->x)*(double)(end->x - point->x) + (double)(end->y - point->y)*(double)(end->y - point->y))*kCost1;
}
 
int Astar::calcF(Point *point)
{
	return point->G + point->H;
}
 
Point *Astar::getLeastFpoint()
{
	if (!openList.empty())
	{
		auto resPoint = openList.front();
		for (auto &point : openList)
		if (point->F<resPoint->F)
			resPoint = point;
		return resPoint;
	}
	return NULL;
}
 
Point *Astar::findPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner)
{
	openList.push_back(new Point(startPoint.x, startPoint.y)); //置入起点,拷贝开辟一个节点，内外隔离
	while (!openList.empty())
	{
		auto curPoint = getLeastFpoint(); //找到F值最小的点
		openList.remove(curPoint); //从开启列表中删除
		closeList.push_back(curPoint); //放到关闭列表
		//1,找到当前周围八个格中可以通过的格子
		auto surroundPoints = getSurroundPoints(curPoint, isIgnoreCorner);
		for (auto &target : surroundPoints)
		{
			//2,对某一个格子，如果它不在开启列表中，加入到开启列表，设置当前格为其父节点，计算F G H
			if (!isInList(openList, target))
			{
				target->parent = curPoint;
 
				target->G = calcG(curPoint, target);
				target->H = calcH(target, &endPoint);
				target->F = calcF(target);
 
				openList.push_back(target);
			}
			//3，对某一个格子，它在开启列表中，计算G值, 如果比原来的大, 就什么都不做, 否则设置它的父节点为当前点,并更新G和F
			else
			{
				int tempG = calcG(curPoint, target);
				if (tempG<target->G)
				{
					target->parent = curPoint;
 
					target->G = tempG;
					target->F = calcF(target);
				}
			}
			Point *resPoint = isInList(openList, &endPoint);
			if (resPoint)
				return resPoint; //返回列表里的节点指针，不要用原来传入的endpoint指针，因为发生了深拷贝
		}
	}
 
	return NULL;
}
 
std::list<Point *> Astar::GetPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner)
{
	Point *result = findPath(startPoint, endPoint, isIgnoreCorner);
	std::list<Point *> path;
	//返回路径，如果没找到路径，返回空链表
	while (result)
	{
		path.push_front(result);
		result = result->parent;
	}
 
	// 清空临时开闭列表，防止重复执行GetPath导致结果异常
	openList.clear();
	closeList.clear();
 
	return path;
}
 
Point *Astar::isInList(const std::list<Point *> &list, const Point *point) const
{
	//判断某个节点是否在列表中，这里不能比较指针，因为每次加入列表是新开辟的节点，只能比较坐标
	for (auto p : list)
	if (p->x == point->x&&p->y == point->y)
		return p;
	return NULL;
}
 
bool Astar::isCanreach(const Point *point, const Point *target, bool isIgnoreCorner) const
{
	if (target->x<0 || target->x>maze.size() - 1
		|| target->y<0 || target->y>maze[0].size() - 1
		|| maze[target->x][target->y] == 1
		|| target->x == point->x&&target->y == point->y
		|| isInList(closeList, target)) //如果点与当前节点重合、超出地图、是障碍物、或者在关闭列表中，返回false
		return false;
	else
	{
		if (abs(point->x - target->x) + abs(point->y - target->y) == 1) //非斜角可以
			return true;
		else
		{
			//斜对角要判断是否绊住
			if (maze[point->x][target->y] == 0 && maze[target->x][point->y] == 0)
				return true;
			else
				return isIgnoreCorner;
		}
	}
}
 
std::vector<Point *> Astar::getSurroundPoints(const Point *point, bool isIgnoreCorner) const
{
	std::vector<Point *> surroundPoints;
 
	for (int x = point->x - 1; x <= point->x + 1; x++)
	for (int y = point->y - 1; y <= point->y + 1; y++)
	if (isCanreach(point, new Point(x, y), isIgnoreCorner))
		surroundPoints.push_back(new Point(x, y));
 
	return surroundPoints;

}

